#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import serial
import math
import time
import numpy as np


class SerialControl:
    def __init__(self, port: str, baudrate: int, timeout: int = 1):
        """
        初始化串口配置
        :param port: 串口号，例如："/dev/ttyUSB0"
        :param baudrate: 波特率，例如：9600
        :param timeout: 超时时间（秒），默认为1秒
        """
        # 初始化串口参数
        self.ser = serial.Serial(
            port=port,
            baudrate=baudrate,
            parity=serial.PARITY_NONE,  # 无校验位
            stopbits=serial.STOPBITS_ONE,  # 1位停止位
            bytesize=serial.EIGHTBITS,  # 8位数据位
            timeout=timeout  # 超时时间
        )

        # 生成舵机ID映射表（0-29转为三位字符表示，如0 → 000，15 → 015，依此类推）
        self.id_map = {str(i): f"{i:03d}" for i in range(0, 30)}

        # 尝试打开串口（自动重试机制）
        while True:
            try:
                if self.ser.is_open:
                    print("串口已打开，正在自检...")
                    time.sleep(1.5)  # 等待自检时间
                    print("自检已完成，准备接受指令...")
                    break
            except serial.SerialException as e:
                print(f"串口打开失败，错误信息：{e}, 请检查串口号和权限。")
                time.sleep(1.5)  # 重试间隔


    def _generate_write_command(self, rads: list, machine: str = "arm") -> str:
        """
        生成写入关节控制命令
        :param rads: 关节角度列表（弧度制）
        :param machine: 控制设备类型，默认为 "arm"
        :return: 控制命令字符串
        """
        if machine == "arm":
            print("Control arm...")
            pwm_list = []
            for i in range(5):
                if i != 3:
                    pwm = 1500 - rads[i] * 4000 / (3 * math.pi)
                else:
                    pwm = rads[i] * 4000 / (3 * math.pi) + 1500
                pwm_list.append(round(pwm))
            command_parts = [f"#{i + 21}P{pwm}T1500!" for i, pwm in enumerate(pwm_list)]
            return "{" + "".join(command_parts) + "}"
        elif machine == "legs":
            print("Control legs...")
            return None


    def _generate_read_command(self, machine: str = "arm") -> list:
        """
        （私有）生成读取关节状态的命令
        :param machine: 控制设备类型，默认为 "arm"
        :return: 读取命令列表
        """
        command_list = []
        if machine == "arm":
            print("read from arm")
            for i in range(5):
                # 生成读取舵机状态的命令（ID 21-25）
                command_list.append(f"#{self.id_map[str(i + 21)]}PRAD!")
        elif machine == "legs":
            print("read from legs")
        else:
            print("Unknown robot part!")

        return command_list

    def send_command(self, rads: list, machine: str = "arm"):
        """
        通过串口发送命令
        :param command: 控制命令字符串
        """
        command = self._generate_write_command(rads=rads, machine=machine)
        self.ser.write(command.encode())
        time.sleep(2)  # 等待舵机响应


    def get_rads_data(self, machine: str = "arm") -> list:
        """
        获取关节角度反馈数据（弧度制）
        :param machine: 控制设备类型，默认为 "arm"
        :return: 关节角度列表（弧度制）
        """
        # 生成读取命令
        read_commands = self._generate_read_command(machine=machine)

        PWMFeedbackList = []
        # 逐条发送读取命令，获取反馈数据
        for cmd in read_commands:
            self.ser.write(cmd.encode())
            time.sleep(0.2)  # 等待舵机响应

            # 读取所有返回数据
            PWMFeedback = self.ser.read_all()
            print(f"type:{type(PWMFeedback)}")
            PWMFeedbackList.append(PWMFeedback.decode('ascii', errors='ignore'))

        # 解析反馈数据
        try:
            return self.parse_PWMFeedbackList(PWMFeedbackList)
        except Exception as e:
            print(f"解析反馈数据时出错：{e}")
            return []


    @staticmethod
    def parse_PWMFeedbackList(PWMFeedbackList: list) -> list:
        """
        解析原始串口反馈数据，转换为关节角度（弧度制）
        :param PWMFeedbackList: 原始反馈数据列表
        :return: 关节角度列表（弧度制）
        """
        PWMList = []
        for item in PWMFeedbackList:
            # 检查数据长度，不足的不处理
            if len(item) < 5:
                continue

            # 提取最后四位作为PWM值
            pwm_str = item[-5:-1]
            if pwm_str.isdigit():
                pwm = int(pwm_str)
                # 检查PWM值是否在有效范围内
                if 500 <= pwm <= 2500:
                    PWMList.append(pwm)

        # 转换PWM值为关节角度
        radList = []
        for idx, pwm in enumerate(PWMList[:5]):  # 只处理前5个关节
            if idx != 3:
                # 标准转换：(中位值1500 - 实际PWM) * 转换系数
                rad = (1500 - pwm) * 3 * math.pi / 4000
            else:
                # 第4关节（索引3）方向相反
                rad = (pwm - 1500) * 3 * math.pi / 4000

            radList.append(rad)

        return radList


    def close_serial(self):
        """
        关闭串口连接
        """
        self.ser.close()
